#include "boundingbox.h"

namespace perception
{
    namespace algorithm
    {

        BoundingBox::BoundingBox() {}

        void BoundingBox::Compute(LidarObject &object)
        {
            // calculate objects width heigh length
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            for (auto &cell : object.cluster_points.points)
            {

                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z);
                max_z = std::max(max_z, cell.z);
            }
            object.x = (min_x + max_x) / 2.0;
            object.y = (min_y + max_y) / 2.0;
            object.z = (max_z + min_z) / 2.0;
            object.z_max = max_z;
            object.z_min = min_z;
        }
        void BoundingBox::ComputeLWH(LidarObject &object)
        {
            // calculate objects width heigh length
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            int intensity = 0;
            for (auto &cell : object.cluster_points.points)
            {
                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z);
                max_z = std::max(max_z, cell.z);
                if (cell.intensity < 30)
                    intensity++;
                // intensity[static_cast<int>(cell.intensity)]++;
            }
            if (float(intensity) / float(object.cluster_points.points.size()) > 0.5)
                object.intensity = 10;
            else
                object.intensity = 50;
            object.x = (min_x + max_x) / 2.0;
            object.y = (min_y + max_y) / 2.0;
            object.z = (max_z + min_z) / 2.0;
            object.length = (min_x == max_x) ? 0.1 : max_x - min_x;
            object.width = (min_y == max_y) ? 0.1 : max_y - min_y;
            object.height = (min_z == max_z) ? 0.1 : max_z - min_z;
            // std::vector<float> c = {min_x, min_y, max_x, max_y};
            // std::vector<float> a = {std::cos(object.heading), -std::sin(object.heading), std::cos(object.heading), -std::sin(object.heading)};
            // std::vector<float> b = {std::sin(object.heading), std::cos(object.heading), std::sin(object.heading), std::cos(object.heading)};
            // std::vector<CornerPoint> corners;

            // corners.push_back(GetCorner(a[1], b[1], c[1], a[2], b[2], c[2]));
            // corners.push_back(GetCorner(a[0], b[0], c[0], a[1], b[1], c[1]));
            // corners.push_back(GetCorner(a[0], b[0], c[0], a[3], b[3], c[3]));
            // corners.push_back(GetCorner(a[3], b[3], c[3], a[2], b[2], c[2]));
            // object.corner_points = corners;
            // float min_dis = 10000.0;
            // int idx = 0;
            // int best_idx = 0;
            // for (auto &corner : corners)
            // {
            //     auto dis = std::sqrt(corner.x * corner.x + corner.y * corner.y);
            //     if (dis < min_dis)
            //     {
            //         min_dis = dis;
            //         object.corner_point = corner;
            //         best_idx = idx;
            //     }
            //     idx++;
            // }
            // // object.corner_point = corners[2];
            // std::vector<CornerPoint> l1l2_list;
            // if (best_idx == 3)
            // {
            //     l1l2_list.push_back(corners[0]);
            // }
            // else
            // {
            //     l1l2_list.push_back(corners[best_idx + 1]);
            // }
            // l1l2_list.push_back(corners[best_idx]);
            // if (best_idx == 0)
            // {
            //     l1l2_list.push_back(corners[3]);
            // }
            // else
            // {
            //     l1l2_list.push_back(corners[best_idx - 1]);
            // }
            // auto l1l2 = l1l2_list;
            // // std::cout << "l1l2 size:" << l1l2.size() << std::endl;
            // double dx = l1l2_list[1].x - l1l2_list[0].x;
            // double dy = l1l2_list[1].y - l1l2_list[0].y;
            // auto L1 = std::pow(std::pow(dx, 2.0) + std::pow(dy, 2.0), 0.5);
            // dx = l1l2_list[1].x - l1l2_list[2].x;
            // dy = l1l2_list[1].y - l1l2_list[2].y;
            // auto L2 = std::pow(std::pow(dx, 2.0) + std::pow(dy, 2.0), 0.5);

            // auto thetaL1 = std::atan2((l1l2[0].y - l1l2[1].y), (l1l2[0].x - l1l2[1].x));

            // auto thetaL2 = std::atan2((l1l2[2].y - l1l2[1].y), (l1l2[2].x - l1l2[1].x));
            // object.thetal1 = thetaL1;
            // object.thetal2 = thetaL2;
            // // std::cout << "L1:" << L1 << " L2:" << L2
            // //           << " length:" << object.length << " width:" << object.width
            // //           << " heading:" << object.heading * rad2deg << std::endl;
            // object.l1 = L1;
            // object.l2 = L2;
            // /*
            // 3  0
            // ____
            // |  |
            // |__|
            // 2  1

            // */

            // if (object.x > 0)
            // {
            //     // 左前
            //     if (object.y > 1.0)
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[1 + 1];
            //         else
            //             object.corner_point = corners[1];
            //     }
            //     // 右前
            //     else if (object.y < -1.0)
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[2 + 1];
            //         else
            //             object.corner_point = corners[2];
            //         // object.corner_point = corners[2];
            //     }
            //     // 前
            //     else
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[1 + 1];
            //         else
            //             object.corner_point = corners[1];
            //     }
            // }
            // else
            // {
            //     // 左前
            //     if (object.y > 1.0)
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[0 + 1];
            //         else
            //             object.corner_point = corners[0];
            //     }
            //     // 右前
            //     else if (object.y < -1.0)
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[0];
            //         else
            //             object.corner_point = corners[3];
            //     }
            //     // 前
            //     else
            //     {
            //         if (object.heading > M_PI_4)
            //             object.corner_point = corners[0 + 1];
            //         else
            //             object.corner_point = corners[0];
            //     }
            // }

            // object.corner_points = cps;
        }
        CornerPoint BoundingBox::GetCorner(float &a0, float &b0, float &c0, float &a1, float &b1, float &c1)
        {
            CornerPoint corner;
            double determinant = a0 * b1 - a1 * b0;
            corner.x = (b1 * c0 - b0 * c1) / determinant;
            corner.y = (a0 * c1 - a1 * c0) / determinant;
            // std::cout << "~~~~~~~~~~~~~~~~######################" << std::endl;
            // std::cout << "tmp[i].x:" << corner.x << " tmp[i].y:" << corner.y << " c0:" << c0 << " c1:" << c1
            //           << " determinantL:" << determinant
            //           << " a0:" << a0 << " a1:" << a1
            //           << " b0:" << b0 << " b1:" << b1
            //           << std::endl;
            return corner;
        }
        void BoundingBox::ComputeCornerPoints(LidarObject &object)
        {
            // calculate objects width heigh length
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            for (auto &cell : object.cells)
            {

                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z_min);
                max_z = std::max(max_z, cell.z_max);
            }
            std::vector<float> c = {min_x, min_y, max_x, max_y};
            std::vector<float> a = {std::cos(object.heading), -std::sin(object.heading), std::cos(object.heading), -std::sin(object.heading)};
            std::vector<float> b = {std::sin(object.heading), std::cos(object.heading), std::sin(object.heading), std::cos(object.heading)};
            std::vector<CornerPoint> corners;

            corners.push_back(GetCorner(a[1], b[1], c[1], a[2], b[2], c[2]));
            corners.push_back(GetCorner(a[0], b[0], c[0], a[1], b[1], c[1]));
            corners.push_back(GetCorner(a[0], b[0], c[0], a[3], b[3], c[3]));
            corners.push_back(GetCorner(a[3], b[3], c[3], a[2], b[2], c[2]));
            float min_dis = 10000.0;
            int idx = 0;
            int best_idx = 0;
            for (auto &corner : corners)
            {
                auto dis = std::sqrt(corner.x * corner.x + corner.y * corner.y);
                if (dis < min_dis)
                {
                    min_dis = dis;
                    object.corner_point = corner;
                    best_idx = idx;
                }
                idx++;
            }
            std::vector<CornerPoint> l1l2_list;
            if (best_idx == 3)
            {
                l1l2_list.push_back(corners[0]);
            }
            else
            {
                l1l2_list.push_back(corners[best_idx + 1]);
            }
            l1l2_list.push_back(corners[best_idx]);
            if (best_idx == 0)
            {
                l1l2_list.push_back(corners[3]);
            }
            else
            {
                l1l2_list.push_back(corners[best_idx - 1]);
            }
            auto l1l2 = l1l2_list;
            // std::cout << "l1l2 size:" << l1l2.size() << std::endl;
            double dx = l1l2_list[1].x - l1l2_list[0].x;
            double dy = l1l2_list[1].y - l1l2_list[0].y;
            auto L1 = std::pow(std::pow(dx, 2.0) + std::pow(dy, 2.0), 0.5);
            dx = l1l2_list[1].x - l1l2_list[2].x;
            dy = l1l2_list[1].y - l1l2_list[2].y;
            auto L2 = std::pow(std::pow(dx, 2.0) + std::pow(dy, 2.0), 0.5);

            auto thetaL1 = std::atan2((l1l2[0].y - l1l2[1].y), (l1l2[0].x - l1l2[1].x));

            auto thetaL2 = std::atan2((l1l2[2].y - l1l2[1].y), (l1l2[2].x - l1l2[1].x));
            object.thetal1 = thetaL1;
            object.thetal2 = thetaL2;
            // std::cout << "L1:" << L1 << " L2:" << L2
            //           << " length:" << object.length << " width:" << object.width
            //           << " heading:" << object.heading * rad2deg << std::endl;
            object.l1 = L1;
            object.l2 = L2;
        }
        void BoundingBox::CoordinateTransformation(CornerPoint &cells, float &rad)
        {
            auto tmp = cells;

            cells.x = tmp.x * std::cos(rad) + tmp.y * std::sin(rad);
            cells.y = tmp.y * std::cos(rad) - tmp.x * std::sin(rad);
        }
    } // namespace perception
} // namespace avos